/*<DOC
        TMatrix math library
        ====================
    Author:  Peter Demin, Moscow, Russia
    Started: 22.02.2010

Class matrix::TMatrix provides access to some matrix operations.
Class looks like boost::numeric::ublas::matrix, but doesn't contain tones of code,
so it compiles faster.

Available operations:
    # Math:
        1. TMatrix add(TMatrix &m1, TMatrix &m2)      - addition
        2. TMatrix sub(TMatrix &m1, TMatrix &m2)      - substraction
        3. TMatrix prod(TMatrix &m1, TMatrix &m2)     - production
        4. TMatrix trans(TMatrix &m)                  - transposition
        5. TMatrix invert(TMatrix &m)                 - invertation
    # Misc
        1. TMatrix column(TMatrix &m, size_t col_id)  - extrude matrix's column to vector
        2. TMatrix row(TMatrix &m, size_t col_id)     - extrude matrix's row to vector

Production can act in 2 ways, depending on passed argument:
    1. Standard matrix production (M = V * V_T).
       Acts when dimensions are: MxK = MxN * NxK
    2. Vector production, where   res_i = v1_i * v2_i.
       Acts when dimensions are: Mx1 = Mx1 * Mx1
If another dimensions will be used, compile error will occur.

Example usage:
|   #include "TMatrix.h"
|   using namespace matrix
|   TMatrix<double, 3, 1> a;
|   TMatrix<double, 1, 3> b;
|   TMatrix<double, 3, 3> c;
|   TMatrix<double, 3, 3> d;
|   a(0) = 1;    b(0, 0) = 4;
|   a(1) = 2;    b(0, 1) = 5;
|   a(2) = 3;    b(0, 2) = 6;
|   for(size_t m = 0; m < c.size1; m++)
|       for(size_t n = 0; n < c.size2; n++)
|           c(m, n) = m * n + n;
|   d = c + a * b;

Constructor gets 3 template arguments:
    1. Type of matrix elements.
    2. Number of rows.
    3. Number of columns.
If number of columns is 1, elements can be accessed, by providing 1 number.

Class TTriangular constructed with TMatrix represents triangular matrix,
(constructor replaces upper-right corner with zeros) that is usefull
for kholetsky decomposition.

Note: Operations with matrices of unsuitable dimensions, will cause compilation error.
DOC>*/

#ifndef TMATRIX_H_INCLUDED
#define TMATRIX_H_INCLUDED

#include <cassert>
#include <math.h>

namespace matrix {
    template<typename T, size_t S1, size_t S2> class TMatrix;

    //=========================================== FUNC prototypes
    template<typename T, size_t S1, size_t S2, size_t S3> inline TMatrix<T, S1, S3> prod(const TMatrix<T, S1, S2> &m1, const TMatrix<T, S2, S3> &m2);
    template<typename T, size_t S1, size_t S2> inline TMatrix<T, S2, S1> trans(const TMatrix<T, S1, S2> &matr);

    template<class M> inline M add(const M &m1, const M &m2);
    template<class M> inline M sub(const M &m1, const M &m2);

    template<typename T, size_t S1, size_t S2> inline TMatrix<T, S1, S2> invert( const TMatrix<T, S1, S2> &input );

    //=========================================== CLASS TMatrix
    template<typename T, size_t S1, size_t S2>
    class TMatrix {
    public:
        TMatrix() {}
        TMatrix(const TMatrix &other) {
            for(size_t m = 0; m < S1; m++)
                for(size_t n = 0; n < S2; n++)
                    (*this)(m, n) = other(m, n);
        }
        inline void zero(void) {
            for(size_t m = 0; m < S1; m++)
                for(size_t n = 0; n < S2; n++)
                    (*this)(m, n) = 0;
        }
        inline void identity(void) {
            for(size_t m = 0; m < S1; m++)
                for(size_t n = 0; n < S2; n++)
                    if(m == n) (*this)(m, n) = 1;
                    else       (*this)(m, n) = 0;
        }

        inline TMatrix &operator= (const TMatrix &other) {
            for(size_t m = 0; m < S1; m++)
                for(size_t n = 0; n < S2; n++)
                    (*this)(m, n) = other(m, n);
            return (*this);
        }

        inline const T& operator() (size_t by_height) const
        { return values[by_height][0]; }
        inline T& operator() (size_t by_height)
        { return values[by_height][0]; }

        inline const T& operator() (size_t by_height, size_t by_width) const
        { return values[by_height][by_width]; }
        inline T& operator() (size_t by_height, size_t by_width)
        { return values[by_height][by_width]; }

        inline TMatrix operator- (const TMatrix &other) const
        { return sub((*this), other); }
        inline TMatrix operator- (const T &other) const
        { return sub((*this), other); }
        inline TMatrix operator+ (const TMatrix &other) const
        { return add((*this), other); }
        template<size_t S3>
        inline TMatrix<T, S1, S3> operator* (const TMatrix<T, S2, S3> &other) const
        { return prod((*this), other); }
        //inline TMatrix<T, 1, 1> operator* (const T &other) const
        //{ return prod((*this), other); }

        public:     // variables
            static const size_t size1 = S1;
            static const size_t size2 = S2;
            typedef T value_type;
        protected:  // variables
            T   values[S1][S2];
    };

    //=========================================== CLASS TTriangular
    template<typename T, size_t S1, size_t S2>
    class TTriangular : public TMatrix<T, S1, S2> {
    public:
        TTriangular(const TMatrix &full) {
            size_t m, n;
            for(m = 0; m < S1; m++) {
                for(n = 0; n <= m; n++)
                    values[m][n] = full(m, n);
                for(n = m+1; n < S2; n++)
                    values[m][n] = 0;
            }
        }
    };

    //=========================================== FUNC column
    template<class M>
    inline TMatrix<typename M::value_type, M::size1, 1> column(const M &matr, size_t column_id) {
        TMatrix<M::value_type, M::size1, 1> col;
        for(size_t rid = 0; rid < matr.size1; rid++)
            col(rid) = matr(rid, column_id);
        return col;
    }

    //=========================================== FUNC row
    template<class M>
    inline TMatrix<typename M::value_type, M::size2, 1> row(const M &matr, size_t row_id) {
        TMatrix<M::value_type, M::size2, 1> row;
        for(size_t cid = 0; cid < matr.size2; cid++)
            row(cid) = matr(row_id, cid);
        return row;
    }

    //=========================================== FUNC prod
    template<typename T, size_t S1, size_t S2, size_t S3>
    inline TMatrix<T, S1, S3> prod(const TMatrix<T, S1, S2> &m1,
                            const TMatrix<T, S2, S3> &m2) {
        TMatrix<T, S1, S3> r;
        size_t m, n, k;
        T sum = 0;
        for(m = 0; m < m1.size1; m++) {
            for(n = 0; n < m2.size2; n++) {
                sum = 0;
                for(k = 0; k < m2.size1; k++) {
                    sum+= m1(m, k) * m2(k, n);
                }
                r(m, n) = sum;
            }
        }
        return r;
    }

    //=========================================== FUNC prod (vectors)
    template<typename T, size_t S1>
    inline T prod(const TMatrix<T, S1, 1> &m1,
           const TMatrix<T, S1, 1> &m2) {
        T r(0);
        for(size_t m = 0; m < m1.size1; m++) {
            r+= m1(m) * m2(m);
        }
        return r;
    }

    //=========================================== FUNC prod (scalar)
    template<typename T, size_t S1, size_t S2>
    inline TMatrix<T, S1, S2> prod(const TMatrix<T, S1, S2> &matr,
                                   const typename T s) {
        TMatrix<T, S1, S2> r;
        for(size_t m = 0; m < matr.size1; m++) {
            for(size_t n = 0; n < matr.size2; n++) {
                r(m, n) = matr(m, n) * s;
            }
        }
        return r;
    }

    //=========================================== FUNC trans
    template<typename T, size_t S1, size_t S2>
    inline TMatrix<T, S2, S1> trans(const TMatrix<T, S1, S2> &matr) {
        TMatrix<T, S2, S1> r;
        size_t m, n;
        for(m = 0; m < matr.size1; m++) {
            for(n = 0; n < matr.size2; n++) {
                r(n, m) = matr(m, n);
            }
        }
        return r;
    }

    //=========================================== FUNC add
    template<class M>
    inline M add(const M &m1, const M &m2) {
        M r;
        size_t m, n;
        for(m = 0; m < m1.size1; m++) {
            for(n = 0; n < m1.size2; n++) {
                r(m, n) = m1(m, n) + m2(m, n);
            }
        }
        return r;
    }

    //=========================================== FUNC sub
    template<class M>
    inline M sub(const M &m1, const M &m2) {
        M r;
        size_t m, n;
        for(m = 0; m < m1.size1; m++) {
            for(n = 0; n < m1.size2; n++) {
                r(m, n) = m1(m, n) - m2(m, n);
            }
        }
        return r;
    }

    //=========================================== FUNC sub
    template<typename T>
    inline TMatrix<T, 1, 1> sub(const TMatrix<T, 1, 1> &m, const T &v) {
        TMatrix<T, 1, 1> r;
        r(0) = m(0) - v;
        return r;
    }

    //=========================================== FUNC cholesky_invert
    template<class M>
    inline M cholesky_invert(M &m) {
        typedef typename M::value_type value_type;
        size_t size = m.size1;
        // determine the inverse of the lower triangular matrix
        for( size_t i = 0; i < size; i++ ) {
            m(i, i) = 1 / m(i, i);
            for( size_t j = i+1; j < size; j++ ) {
                value_type elem(0);
                for( size_t k = i; k < j; k++ ) {
                    elem -= m(j, k) * m(k, i);
                }
                m(j, i) = elem / m(j, j);
            }
        }
        // multiply the upper and lower inverses together
        TTriangular<value_type, M::size1, M::size1> t(m);
        return prod<value_type, m.size1, m.size2, m.size1>(trans(t), t);
    }

    //=========================================== FUNC cholesky_decompose
    template <typename T, size_t S1>
    inline TMatrix<T, S1, S1> cholesky_decompose(const TMatrix<T, S1, S1> &A) {
        // Cholesky decomposition
        // Algorithm from http://en.wikipedia.org/wiki/Cholesky_decomposition
        TMatrix<T, S1, S1> L;
        const size_t n = S1;
        size_t i, j, k;
        T sL_k, sqL_ik, L_ik, L_jj;
        L.zero();
        for(i = 0; i < n; i++) {
            // Eval [0..i-1; i]
            for(j = 0; j < i; j++) {
                sL_k = 0;
                for(k = 0; k < j; k++) {
                    sL_k+= L(i, k) * L(j, k);
                }
                L_jj = L(j, j);
                assert(L_jj != 0);
                L(i, j) = (A(i, j) - sL_k) / L_jj;
            }
            // Eval [i; i]
            sqL_ik = 0;
            for(k = 0; k < i; k++) {
                L_ik = L(i, k);
                sqL_ik+= L_ik * L_ik;
            }
            sqL_ik = A(i, i) - sqL_ik;
            assert(sqL_ik > 0);
            L(i, i) = sqrt(sqL_ik);
        }
        return L;
    }

    //=========================================== FUNC invert
    template<typename T, size_t S1, size_t S2>
    inline TMatrix<T, S1, S2> invert( const TMatrix<T, S1, S2> &input ) {
        TMatrix<T, S1, S2> triangular = cholesky_decompose(input);
        return cholesky_invert(triangular);
    }
}

#ifdef _DEBUG
#include "Tests\Test_TMatrix.h"
#endif

#endif